

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("/media/zwh/T7/ZWH/bags/dataset/semantic/sequences/00/velodyne/80500000.pcd", *cloud) == -1) //*Žò¿ªµãÔÆÎÄŒþ
    {
        PCL_ERROR("Couldn't read file.pcd\n");
        return (-1);
    }

    std::string out_file = "/media/zwh/T7/ZWH/bags/dataset/semantic/sequences/00/velodyne/80500000.bin";
    std::ofstream bin_file(out_file.c_str(), std::ios::out | std::ios::binary);

    for (int j = 0; j < cloud->size(); j++)
    {

        bin_file.write((char *)&cloud->at(j).x, sizeof(cloud->at(j).x));
        bin_file.write((char *)&cloud->at(j).y, sizeof(cloud->at(j).y));
        bin_file.write((char *)&cloud->at(j).z, sizeof(cloud->at(j).z));
        bin_file.write((char *)&cloud->at(j).intensity, sizeof(cloud->at(j).intensity));
    }
    bin_file.close();

    return 0;
}
